#include "ros/ros.h"
#include "information_publisher/status.h"
#include <std_msgs/String.h>

// ANSI终端控制码（无需额外头文件，直接通过字符串实现）
#define CLR_RESET "\033[0m"
#define CLR_RED "\033[31;1m"
#define CLR_GREEN "\033[32;1m"
#define CLR_YELLOW "\033[33;1m"
#define CLR_BLUE "\033[34;1m"
#define CLR_PURPLE "\033[35;1m"
#define CLR_CYAN "\033[36;1m"
#define STYLE_BOLD "\033[1m"
#define STYLE_UNDERLINE "\033[4m"
#define CLEAR_SCREEN "\033[2J\033[H"  // 清屏并将光标移至左上角

// 装饰性分隔符
const char* LINE_TOP = "==============================================";
const char* LINE_MID = "----------------------------------------------";

void callback(const information_publisher::status::ConstPtr& msg) {
    // 清屏刷新，避免信息堆积
    ROS_INFO_STREAM(CLEAR_SCREEN);

    // 标题栏
    ROS_INFO_STREAM(CLR_CYAN << LINE_TOP << CLR_RESET);
    ROS_INFO_STREAM(CLR_CYAN << STYLE_BOLD << "          松灵小车状态监控面板          " << CLR_RESET);
    ROS_INFO_STREAM(CLR_CYAN << LINE_TOP << CLR_RESET << "\n");

    // 1. 灯光模式（蓝色组）
    std::string lmode;
    if (msg->light_mode == 1)      lmode = "灯光打开";
    else if (msg->light_mode == 2) lmode = "灯光打开";
    else if (msg->light_mode == 3) lmode = "灯光关闭";
    else if (msg->light_mode == 4) lmode = "灯光编程模式";
    else                           lmode = "未知模式";

    ROS_INFO_STREAM(CLR_BLUE << STYLE_BOLD << "[灯光系统状态]" << CLR_RESET);
    ROS_INFO_STREAM("  " << CLR_BLUE << "模式: " << STYLE_BOLD << lmode << CLR_RESET << "\n");

    // 2. 电池信息（绿色/红色预警）
    ROS_INFO_STREAM(CLR_GREEN << STYLE_BOLD << "[电源系统状态]" << CLR_RESET);
    const char* bat_color = (msg->battery_voltage < 11.5) ? CLR_RED : CLR_GREEN;
    ROS_INFO_STREAM("  " << CLR_GREEN << "电池电压: " << bat_color << msg->battery_voltage << "V" 
                   << (msg->battery_voltage < 11.5 ? "  ⚠️ 低电量" : "") << CLR_RESET);
    ROS_INFO_STREAM("  " << CLR_GREEN << "驱动器电压: " << msg->driver_voltage << "V" << CLR_RESET << "\n");

    // 3. 电机状态（黄色组）
    ROS_INFO_STREAM(CLR_YELLOW << STYLE_BOLD << "[动力系统状态]" << CLR_RESET);
    ROS_INFO_STREAM("  " << CLR_YELLOW << "电机数量: " << static_cast<int>(msg->motor_num) << "个" << CLR_RESET);
    ROS_INFO_STREAM("  " << CLR_YELLOW << "电机电流: " << msg->motor_current << "A" << CLR_RESET);
    ROS_INFO_STREAM("  " << CLR_YELLOW << "电机转速: " << msg->motor_rpm << "rpm" << CLR_RESET);
    ROS_INFO_STREAM("  " << CLR_YELLOW << "脉冲计数: " << msg->motor_pulses << CLR_RESET);
    
    const char* temp_color = (msg->motor_temperature > 55) ? CLR_RED : CLR_YELLOW;
    ROS_INFO_STREAM("  " << CLR_YELLOW << "电机温度: " << temp_color << msg->motor_temperature << "℃" 
                   << (msg->motor_temperature > 55 ? "  ⚠️ 过热" : "") << CLR_RESET << "\n");

    // 4. 驱动器状态（紫色组）
    ROS_INFO_STREAM(CLR_PURPLE << STYLE_BOLD << "[驱动控制状态]" << CLR_RESET);
    const char* driver_state = (msg->driver_state == 0) ? CLR_GREEN"正常" : CLR_RED"异常";
    ROS_INFO_STREAM("  " << CLR_PURPLE << "驱动器状态: " << driver_state << CLR_RESET);
    ROS_INFO_STREAM("  " << CLR_PURPLE << "驱动器温度: " << msg->driver_temperature << "℃" << CLR_RESET << "\n");

    // 5. 运动状态（青色组）
    ROS_INFO_STREAM(CLR_CYAN << STYLE_BOLD << "[运动性能状态]" << CLR_RESET);
    ROS_INFO_STREAM("  " << CLR_CYAN << "线速度: " << (msg->linear >= 0 ? "前进 " : "后退 ") 
                   << fabs(msg->linear) << "m/s" << CLR_RESET);
    ROS_INFO_STREAM("  " << CLR_CYAN << "角速度: " << (msg->angular >= 0 ? "逆时针 " : "顺时针 ") 
                   << fabs(msg->angular) << "rad/s" << CLR_RESET << "\n");

    // 底部时间线
    ROS_INFO_STREAM(CLR_CYAN << LINE_MID << CLR_RESET);
    ROS_INFO_STREAM(STYLE_BOLD << "最后更新: " << ros::Time::now() << CLR_RESET);
    ROS_INFO_STREAM(CLR_CYAN << LINE_MID << CLR_RESET);
}

int main(int argc, char **argv) {
    setlocale(LC_CTYPE, "zh_CN.utf8");  // 保持中文显示

    ros::init(argc, argv, "information_subscriber");
    ros::NodeHandle nh;
  
    ros::Subscriber sub = nh.subscribe<information_publisher::status>("/scout_status", 10, callback);

    ros::spin();
    return 0;
}
